#include "matrix.h"
#include <math.h>

MATRIX3X3F::MATRIX3X3F()
{
	_M11 = _M12 = _M13 = 
	_M21= _M22= _M23 = 
	_M31= _M32= _M33= 0; 
}	

MATRIX3X3F::MATRIX3X3F(const Matrix4f* pOrrientation)
{
	_M11 = pOrrientation->_M11;  
	_M12 = pOrrientation->_M12;  
	_M13 = pOrrientation->_M13;  

	_M21 = pOrrientation->_M21;  
	_M22 = pOrrientation->_M22;  
	_M23 = pOrrientation->_M23;  

	_M31 = pOrrientation->_M31;  
	_M32 = pOrrientation->_M32;  
	_M33 = pOrrientation->_M33;  
}


Vector3f MATRIX3X3F::operator * (Vector3f &op) const
{
	const float x = (_M11 * op.x) + (_M12 * op.y) + (_M13 * op.z); 
	const float y = (_M21 * op.x) + (_M22 * op.y) + (_M23 * op.z); 
	const float z = (_M31 * op.x) + (_M32 * op.y) + (_M33 * op.z); 
	
	return Vector3f (x,y,z); 
}

MATRIX3X3F MATRIX3X3F::transpose(MATRIX3X3F* pIn)
{
	MATRIX3X3F result; 

	result._M11 = pIn->_M11; 
	result._M12 = pIn->_M21; 
	result._M13 = pIn->_M31; 

	result._M21 = pIn->_M12; 
	result._M22 = pIn->_M22; 
	result._M23 = pIn->_M32; 

	result._M31 = pIn->_M13; 
	result._M32 = pIn->_M23; 
	result._M33 = pIn->_M33; 

	return result; 
}

Matrix4f Matrix4f::identity()
{ 
	return Matrix4f(1.0F,0.0F,0.0F,0.0F,
					0.0F,1.0F,0.0F,0.0F,
					0.0F,0.0F,1.0F,0.0F,
					0.0F,0.0F,0.0F,1.0F); 
}


Matrix4f::Matrix4f(void)
{
	_M11 = _M12 = _M13 = _M14 = 
	_M21 = _M22 = _M23 = _M24 = 
	_M31 = _M32 = _M33 = _M34 = 
	_M41 = _M42 = _M43 = _M44 = 0.0f; 
}

Matrix4f::Matrix4f
(
	float	_11,float	_12,float	_13,float	_14,
	float	_21,float	_22,float	_23,float	_24,
	float	_31,float	_32,float	_33,float	_34,
	float	_41,float	_42,float	_43,float	_44
)
{
	_M11 = _11; 
	_M12 = _12; 
	_M13 = _13; 
	_M14 = _14; 

	_M21 = _21; 
	_M22 = _22; 
	_M23 = _23; 
	_M24 = _24; 

	_M31 = _31; 
	_M32 = _32; 
	_M33 = _33; 
	_M34 = _34; 

	_M41 = _41; 
	_M42 = _42; 
	_M43 = _43; 
	_M44 = _44; 
}

Matrix4f::Matrix4f(float M[16])
{
	_M11 = M[0]; 
	_M12 = M[1]; 
	_M13 = M[2]; 
	_M14 = M[3]; 
	_M21 = M[4]; 
	_M22 = M[5]; 
	_M23 = M[6]; 
	_M24 = M[7]; 
	_M31 = M[8]; 
	_M32 = M[9]; 
	_M33 = M[10]; 
	_M34 = M[11]; 
	_M41 = M[12]; 
	_M42 = M[13]; 
	_M43 = M[14]; 
	_M44 = M[15]; 
}
Matrix4f::Matrix4f(float M[4][4])
{
	_M11 = M[0][0]; 
	_M12 = M[0][1]; 
	_M13 = M[0][2]; 
	_M14 = M[0][3]; 
	_M21 = M[1][0]; 
	_M22 = M[1][1]; 
	_M23 = M[1][2]; 
	_M24 = M[1][3]; 
	_M31 = M[2][0]; 
	_M32 = M[2][1]; 
	_M33 = M[2][2]; 
	_M34 = M[2][3]; 
	_M41 = M[3][0]; 
	_M42 = M[3][1]; 
	_M43 = M[3][2]; 
	_M44 = M[3][3]; 
}

Matrix4f::Matrix4f(float M[9], float T[3])
{
	_M11 = M[0]; 
	_M12 = M[1]; 
	_M13 = M[2]; 
	_M14 = 0.0F; 
	_M21 = M[3]; 
	_M22 = M[4]; 
	_M23 = M[5]; 
	_M24 = 0.0F; 
	_M31 = M[6]; 
	_M32 = M[7]; 
	_M33 = M[8]; 
	_M34 = 0.0F; 

	_M41 = T[0]; 
	_M42 = T[1]; 
	_M43 = T[2]; 
	_M44 = 1.0F; 
}


Matrix4f 
Matrix4f::ProjectionMatrix(const float near_plane,const float far_plane,
							 const float fov_horiz,const float aspect)
{
    float    h, w, Q; 
	const float fov_vert = fov_horiz / ((aspect == 0) ? 1 : aspect); 

    w = (float)1/tan(fov_horiz*0.5);   // 1/tan(x) == cot(x)
    h = (float)1/tan(fov_vert*0.5);    // 1/tan(x) == cot(x)
    Q = far_plane/(far_plane - near_plane); 

    Matrix4f ret(0,0,0,0,
		0,0,0,0,
		0,0,0,0,
		0,0,0,0); 

	ret._M11 = w; 
	ret._M22 = h; 
	ret._M33 = -Q; 
	ret._M43 = -Q*near_plane; 
	ret._M34 = -1; 
    return ret; 
}
Vector3f Matrix4f::operator * (Vector3f &op) const
{
	const float x = (_M11 * op.x) + (_M12 * op.y) + (_M13 * op.z) + _M14; 
	const float y = (_M21 * op.x) + (_M22 * op.y) + (_M23 * op.z) + _M24; 
	const float z = (_M31 * op.x) + (_M32 * op.y) + (_M33 * op.z) + _M34; 
	
	return Vector3f (x,y,z); 
}

Vector3f Matrix4f::operator * (Vector3f &op)
{
	const float x = (_M11 * op.x) + (_M12 * op.y) + (_M13 * op.z) + _M14; 
	const float y = (_M21 * op.x) + (_M22 * op.y) + (_M23 * op.z) + _M24; 
	const float z = (_M31 * op.x) + (_M32 * op.y) + (_M33 * op.z) + _M34; 
	
	return Vector3f (x,y,z); 
}

Matrix4f Matrix4f::operator*(Matrix4f &op)
{
	Matrix4f M; 

	M._M11 = (_M11 * op._M11) + (_M21 * op._M12) + (_M31 * op._M13) + (_M41 * op._M14); 
	M._M21 = (_M11 * op._M21) + (_M21 * op._M22) + (_M31 * op._M23) + (_M41 * op._M24); 
	M._M31 = (_M11 * op._M31) + (_M21 * op._M32) + (_M31 * op._M33) + (_M41 * op._M34); 
	M._M41 = (_M11 * op._M41) + (_M21 * op._M42) + (_M31 * op._M43) + (_M41 * op._M44); 

	M._M12 = (_M12 * op._M11) + (_M22 * op._M12) + (_M32 * op._M13) + (_M42 * op._M14); 
	M._M22 = (_M12 * op._M21) + (_M22 * op._M22) + (_M32 * op._M23) + (_M42 * op._M24); 
	M._M32 = (_M12 * op._M31) + (_M22 * op._M32) + (_M32 * op._M33) + (_M42 * op._M34); 
	M._M42 = (_M12 * op._M41) + (_M22 * op._M42) + (_M32 * op._M43) + (_M42 * op._M44); 
	
	M._M13 = (_M13 * op._M11) + (_M23 * op._M12) + (_M33 * op._M13) + (_M43 * op._M14); 
	M._M23 = (_M13 * op._M21) + (_M23 * op._M22) + (_M33 * op._M23) + (_M43 * op._M24); 
	M._M33 = (_M13 * op._M31) + (_M23 * op._M32) + (_M33 * op._M33) + (_M43 * op._M34); 
	M._M43 = (_M13 * op._M41) + (_M23 * op._M42) + (_M33 * op._M43) + (_M43 * op._M44); 

	M._M14 = (_M14 * op._M11) + (_M24 * op._M12) + (_M34 * op._M13) + (_M44 * op._M14); 
	M._M24 = (_M14 * op._M21) + (_M24 * op._M22) + (_M34 * op._M23) + (_M44 * op._M24); 
	M._M34 = (_M14 * op._M31) + (_M24 * op._M32) + (_M34 * op._M33) + (_M44 * op._M34); 
	M._M44 = (_M14 * op._M41) + (_M24 * op._M42) + (_M34 * op._M43) + (_M44 * op._M44); 

	return M; 
}
Matrix4f Matrix4f::operator=(const Matrix4f &op)
{
	Matrix4f M; 

	_M11 = op._M11; 
	_M12 = op._M12; 
	_M13 = op._M13; 
	_M14 = op._M14; 

	_M21 = op._M21; 
	_M22 = op._M22; 
	_M23 = op._M23; 
	_M24 = op._M24; 

	_M31 = op._M31; 
	_M32 = op._M32; 
	_M33 = op._M33; 
	_M34 = op._M34; 

	_M41 = op._M41; 
	_M42 = op._M42; 
	_M43 = op._M43; 
	_M44 = op._M44; 

	return *this; 
}

Matrix4f Matrix4f::scale(const float scale)
{
	return Matrix4f(	scale, 0, 0, 0, 
						0, scale, 0, 0,
						0, 0, scale, 0,
						0,0,0,1); 
}

Matrix4f Matrix4f::rotateX(const float angle)
{
	return Matrix4f(	1.0f, 0, 0, 0,
						0, cosf(angle), sinf(angle), 0,
						0, -sinf(angle), cosf(angle), 0,
						0,0,0,1); 
}

Matrix4f Matrix4f::rotateY(const float angle)
{
	return Matrix4f(	cosf(angle), 0, -sinf(angle), 0, 
						0, 1.0f, 0, 0,
						sinf(angle), 0, cosf(angle), 0,
						0,0,0,1); 
}

Matrix4f Matrix4f::rotateZ(const float angle)
{
	return Matrix4f(	cosf(angle), sinf(angle), 0, 0,
						-sinf(angle), cosf(angle), 0, 0,
						0, 0.0f, 1.0f, 0,
						0,0,0,1); 
}
Matrix4f Matrix4f::translate(const Vector3f& vec)
{
	return Matrix4f(	1.0f, 0, 0, 0, 
						0, 1.0f, 0, 0,
						0, 0, 1.0f, 0,
						vec.x,vec.y,vec.z,1); 
}

void Matrix4f::invert(const Matrix4f* pIn,Matrix4f* pOut)
{
	float fA0 = pIn->_M11*pIn->_M22 - pIn->_M21*pIn->_M21; 
    float fA1 = pIn->_M11*pIn->_M23 - pIn->_M31*pIn->_M21; 
    
	float fA2 = pIn->_M11*pIn->_M24 - pIn->_M14*pIn->_M21; 
    float fA3 = pIn->_M12*pIn->_M23 - pIn->_M13*pIn->_M22; 
    
	float fA4 = pIn->_M12*pIn->_M24 - pIn->_M14*pIn->_M22; 
    float fA5 = pIn->_M13*pIn->_M24 - pIn->_M14*pIn->_M23; 
    float fB0 = pIn->_M31*pIn->_M42 - pIn->_M32*pIn->_M41; 
    float fB1 = pIn->_M31*pIn->_M43 - pIn->_M33*pIn->_M41; 
    float fB2 = pIn->_M31*pIn->_M44 - pIn->_M34*pIn->_M41; 
    float fB3 = pIn->_M32*pIn->_M43 - pIn->_M33*pIn->_M42; 
    float fB4 = pIn->_M32*pIn->_M44 - pIn->_M34*pIn->_M42; 
    float fB5 = pIn->_M33*pIn->_M44 - pIn->_M34*pIn->_M43; 

    float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0; 
    if (fabs(fDet) <= 0.00001f)
    {
		*pOut = Matrix4f::identity(); 
		return; 
    }

    pOut->_M11 = + pIn->_M22*fB5 - pIn->_M23*fB4 + pIn->_M24*fB3; 
    pOut->_M22 = - pIn->_M21*fB5 + pIn->_M23*fB2 - pIn->_M24*fB1; 
    pOut->_M31 = + pIn->_M21*fB4 - pIn->_M22*fB2 + pIn->_M24*fB0; 
    pOut->_M41 = - pIn->_M21*fB3 + pIn->_M22*fB1 - pIn->_M23*fB0; 
    pOut->_M12 = - pIn->_M12*fB5 + pIn->_M13*fB4 - pIn->_M14*fB3; 
    pOut->_M22 = + pIn->_M11*fB5 - pIn->_M13*fB2 + pIn->_M14*fB1; 
    pOut->_M32 = - pIn->_M11*fB4 + pIn->_M12*fB2 - pIn->_M14*fB0; 
    pOut->_M42 = + pIn->_M11*fB3 - pIn->_M12*fB1 + pIn->_M13*fB0; 
    pOut->_M13 = + pIn->_M42*fA5 - pIn->_M43*fA4 + pIn->_M44*fA3; 
    pOut->_M23 = - pIn->_M41*fA5 + pIn->_M43*fA2 - pIn->_M44*fA1; 
    pOut->_M33 = + pIn->_M41*fA4 - pIn->_M42*fA2 + pIn->_M44*fA0; 
    pOut->_M44 = - pIn->_M41*fA3 + pIn->_M42*fA1 - pIn->_M43*fA0; 
    pOut->_M14 = - pIn->_M32*fA5 + pIn->_M33*fA4 - pIn->_M34*fA3; 
    pOut->_M24 = + pIn->_M31*fA5 - pIn->_M33*fA2 + pIn->_M34*fA1; 
    pOut->_M34 = - pIn->_M31*fA4 + pIn->_M32*fA2 - pIn->_M34*fA0; 
    pOut->_M44 = + pIn->_M31*fA3 - pIn->_M32*fA1 + pIn->_M33*fA0; 

    float fInvDet = ((float)1.0)/fDet; 
    pOut->_M11 *= fInvDet; 
    pOut->_M12 *= fInvDet; 
    pOut->_M13 *= fInvDet; 
    pOut->_M14 *= fInvDet; 
    pOut->_M21 *= fInvDet; 
    pOut->_M22 *= fInvDet; 
    pOut->_M23 *= fInvDet; 
    pOut->_M24 *= fInvDet; 
    pOut->_M31 *= fInvDet; 
    pOut->_M32 *= fInvDet; 
    pOut->_M33 *= fInvDet; 
    pOut->_M34 *= fInvDet; 
    pOut->_M41 *= fInvDet; 
    pOut->_M42 *= fInvDet; 
    pOut->_M43 *= fInvDet; 
    pOut->_M44 *= fInvDet; 
}